//
// Created by 32513 on 25-4-9.
//

#include "cmd.h"

#include <stdint.h>
#include <usart.h>

#include "cmsis_os2.h"
#include "rc_sbus.h"
#include "Servo.h"
//DEBUG: TEST_Value
int8_t DEBUG_TEST_FLAG = 0;
float DEBUG_TEST_Alpha;
float DEBUG_TEST_front_speed;
int8_t DEBUG_TEST_Put_Down;
int8_t DEBUG_TEST_Put_Up;
int8_t DEBUG_TEST_Shoot;


/*****捡球逻辑变量*******/
int8_t Ball_Now_In_Car_Flag = 0;
int8_t Ball_Now_Front_Kuang_Flag = 0;
int8_t Ball_Should_In_Car_Flag = 1;
int8_t Car_Do_With_Ball_Mode = Get_Ball_Mode;
int8_t Car_Next_Do_With_Ball_Mode = Put_Ball_Mode;

enum
{
    Front = 1,
    Back = 2,
    Left = 3,
    Right = 4
};

enum
{
    Run_As_Cow = 1,
    Normal = 2,

};


uint8_t GET_BALL_MODE = 0;  //1为己方球，2为正常救援，3为救援危险目标
int8_t First_Time = 0;
 int8_t Start_Flag = 0;
int8_t Back_Flag = 0;
int8_t Move_Back_Flag = 0;
/******下发到example的运动指令*****/
extern float vw;
extern float front_speed;
extern float mv_alpha;
extern int8_t Car_Stop;

/******通讯相关*******/
extern uint32_t mv1_last_data_time;
extern float mv1_alpha;
extern float mv1_front_speed;
extern int8_t mv1_uart_live;
extern int16_t Ball_Y_Val;
extern int16_t Last_Ball_Y_Val;

extern uint32_t mv2_last_data_time;
extern float mv2_alpha;
extern float mv2_front_speed;
extern int8_t mv2_uart_live;
extern int16_t Kuang_Y_Val;
extern int16_t Last_Kuang_Y_Val;

#define NOW 0
#define LAST 1
extern  rc_obj_t rc_obj[2];   // [0]:当前数据NOW,[1]:上一次的数据LAST


//CMD控制逻辑
void Car_Action()
{

    // if(First_Time == 0)
    // {
    //     HAL_UART_Transmit(&huart5,&GET_BALL_MODE,1,100);
    //
    // }


    if(Ball_Y_Val > 150 /*|| Last_Ball_Y_Val >240*/ && mv1_uart_live == 1)  //活着（在识别时发的消息才算），shoot后会清0，重新检测
    {
        Ball_Now_In_Car_Flag = 1;
        Put_Down();
        Car_Stop = 1;
        front_speed = 0;
        mv_alpha = 0;

        osDelay(500);
        // Car_Stop = 0;
    }
    if( mv2_alpha + 80  > 161  /*&& Last_Kuang_Y_Val == 0*/ && mv2_uart_live == 1 && Ball_Now_In_Car_Flag == 1)   //活着（在识别时发的消息才算），shoot后会清0，重新检测
    {
        Ball_Now_Front_Kuang_Flag = 1;
    }
    if(Car_Do_With_Ball_Mode == Get_Ball_Mode && Ball_Now_In_Car_Flag == 1)  //捡球模式，球在车里，那就放球
    {
        Car_Do_With_Ball_Mode = Put_Ball_Mode;
    }
    if(Car_Do_With_Ball_Mode == Put_Ball_Mode && Ball_Now_Front_Kuang_Flag == 1 && Ball_Now_In_Car_Flag == 1)
    {
        Car_Do_With_Ball_Mode = Shoot_Ball;
        Ball_Now_Front_Kuang_Flag = 0;  //清零
    }

    if(Start_Flag == Run_As_Cow)
    {
        front_speed = 7.5;
    }
    else if(Move_Back_Flag == 1)
    {
        front_speed = -5;
    }
    else if(Start_Flag == Normal)
    {
        switch (Car_Do_With_Ball_Mode)
        {
        case Get_Ball_Mode:
            {
                if(mv1_uart_live == 1 )
                {
                    mv_alpha = mv1_alpha/2.0f;
                    front_speed = mv1_front_speed/2.0f;

                    //角度偏差过大只调角度，角度偏差小才前进靠经目标
                    if(mv1_alpha > 30 || mv1_alpha < -30)
                        front_speed = 2;  //辅助转向;

                }


                if(mv1_uart_live == 0 )  //没找到目标自�?
                {
                    mv_alpha = -66;
                    // mv_alpha = 0;
                    front_speed = 0;
                }
                if(front_speed > 5)
                    front_speed = 5;
                if(front_speed < -5)
                    front_speed = -5;
                break;
            }

        case Put_Ball_Mode:
            {
                if(mv2_uart_live == 1 )
                {
                    mv_alpha = mv2_alpha;
                    front_speed = mv2_front_speed;

                    //角度偏差过大只调角度，角度偏差小才前进靠经目标
                    if(mv2_alpha > 10 || mv2_alpha < -10)
                        front_speed = 2;
                }
                if(mv2_uart_live == 0 )  //无数据则自转
                {
                    mv_alpha = -66;
                    // mv_alpha = 0;
                    front_speed = 0;
                }
                break;
            }
        case Shoot_Ball:
            {
                mv_alpha = mv_alpha;
                // front_speed = -5;
                Put_Up();
                osDelay(300);
                Back_Flag = 1;
                Car_Next_Do_With_Ball_Mode = Get_Ball_Mode;
                First_Time ++;

                break;
            }
        }
        // if(front_flag == 1)


        if(Car_Next_Do_With_Ball_Mode == Get_Ball_Mode)
        {
            // front_speed = -10;
            Car_Do_With_Ball_Mode = Get_Ball_Mode;
            Car_Next_Do_With_Ball_Mode = Put_Ball_Mode;
            Ball_Now_Front_Kuang_Flag = 0;
            Ball_Now_In_Car_Flag = 0;
        }

        // Last_Kuang_Y_Val = Kuang_Y_Val;
        Last_Ball_Y_Val = Ball_Y_Val;
    }




    /*DEBUG:TEST CODE BEGIN*/
    if(rc_obj[NOW].sw5 < 700 )  //SWC向上，失能
    {
        front_speed = 0;
        mv_alpha = 0;
    }
    else if(rc_obj[NOW].sw5 > 700 && rc_obj[NOW].sw5 < 1500)  //SWC在中间手动，在下面不改变变量，为自动
        {
        front_speed = -rc_obj[NOW].ch3/100.0f;
        if(front_speed > 5)
            front_speed = 5;
        if(front_speed < -5)
            front_speed = -5;

        if(rc_obj[NOW].ch2 < -150 || rc_obj[NOW].ch2 > 150)
            front_speed = rc_obj[NOW].ch2/50.0f;


        mv_alpha = -rc_obj[NOW].ch4/10.0f;  //直接给到alpha，目前是相对角度，所以会一直转

        if(rc_obj[NOW].ch1 < -200 || rc_obj[NOW].ch1 > 200)
            mv_alpha = -rc_obj[NOW].ch1/5.0f;


        if(rc_obj[NOW].sw3 < 1000)
            Put_Up();
        else if(rc_obj[NOW].sw3 > 1000)
            Put_Down();
    }

    if(DEBUG_TEST_FLAG!= 0 )
    {
        front_speed = DEBUG_TEST_front_speed;
        mv_alpha = DEBUG_TEST_Alpha;  //直接给到alpha，目前是相对角度，所以会一直转
        if(DEBUG_TEST_Put_Up)
            Put_Up();
        if(DEBUG_TEST_Put_Down)
            Put_Down();
        if(DEBUG_TEST_Shoot)
            Shoot();
    }
    // mv_alpha = mv1_alpha;
    // front_speed = mv1_front_speed;
    /*DEBUG:TEST CODE END*/
}



